#include "ros/ros.h"
#include "cyberarm_controller/pp_msg.h"
#include "cyberarm_controller/arm_state_msg.h"
#include "get_state.hpp"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ROS_INFO("motor pos mode init.");
    ros::init(argc, argv, "motor_pos_mode_init_node");
    
    ros::NodeHandle nh;
    ros::Subscriber state_msg_sub = nh.subscribe("arm_state", 1000, GetState);
    ros::Publisher pub = nh.advertise<cyberarm_controller::pp_msg>("pp_mode", 10); // 位置
    cyberarm_controller::pp_msg t_message[4];

    ros::Rate wait(1);
    wait.sleep();

    while(current_state.id[0] != 1 || current_state.id[1] != 2 || current_state.id[2] != 3 || current_state.id[3] != 4)
    {
        ros::spinOnce();
    }
    ROS_INFO("start_state: %f, %f, %f, %f", current_state.pos_estimate[0], current_state.pos_estimate[1], current_state.pos_estimate[2], current_state.pos_estimate[3]);
    
    double vel, limit_cur;
    if(argc <= 1)
    {
        vel = 200;
        limit_cur = 27;
    }
    else
    {
        vel = std::stof(argv[1]);
        limit_cur = std::stof(argv[2]);
    }
    nh.setParam("mode_init", true);

    int times = 0;
    ros::Rate r(20);
    while(ros::ok() && times < 10)
    {
        times++;
        for(unsigned int i = 0; i < MOTOR_NUM; i++)
        {
            t_message[i].id = i + 1;
            t_message[i].cmd_mode = 18;
            t_message[i].Vel = vel;
            t_message[i].limit_cur = limit_cur;
            t_message[i].Pos = current_state.pos_estimate[i];
            pub.publish(t_message[i]);
        }
        r.sleep();
    }
    

    ROS_INFO("send msg: %f, %f, %f, %f", t_message[0].Pos, t_message[1].Pos, t_message[2].Pos, t_message[3].Pos);

    wait.sleep();
    nh.setParam("mode_init", false); //整型
    ros::spinOnce();
    
    return 0;
}
